from pyproj import Proj, transform

# 定义UTM投影，WGS84坐标
wgs84 = Proj(proj="latlong", datum="WGS84")
utm_zone50 = Proj(proj="utm", zone=50, datum="WGS84")  # 北京位于50号带

# 输入文件路径
input_file = r"D:\files\农业机器人任务规划研究\ros_workspace\navigation2_tutorials\my_test_dir\gps\all_process\selected_lines.txt"
output_file = r"D:\files\农业机器人任务规划研究\ros_workspace\navigation2_tutorials\my_test_dir\gps\all_process\utm_coordinates.txt"

# 读取文件内容
with open(input_file, "r", encoding="utf-8") as f:
    lines = f.readlines()

# 转换经纬度并保留原格式
output_lines = []
for line in lines:
    stripped_line = line.strip()
    if not stripped_line:  # 保留空行
        output_lines.append("\n")
        continue
    try:
        # 解析经纬度
        coords = stripped_line.split(",")
        lon1, lat1, lon2, lat2 = map(float, coords)
        
        # 转换为UTM坐标
        utm_x1, utm_y1 = transform(wgs84, utm_zone50, lon1, lat1)
        utm_x2, utm_y2 = transform(wgs84, utm_zone50, lon2, lat2)
        
        # 格式化并添加到输出
        output_lines.append(f"{utm_x1:.6f},{utm_y1:.6f},{utm_x2:.6f},{utm_y2:.6f}\n")
    except Exception as e:
        print(f"Error processing line: {line}. Error: {e}")
        output_lines.append(line)  # 保留原始行以防错误

# 写入结果到文件
with open(output_file, "w", encoding="utf-8") as f:
    f.writelines(output_lines)

print(f"UTM coordinates written to {output_file}")
